#ifndef __KALFI__
#define __KALFI__
#include "matmatrix.h"
/* kalman filter model extensions change number of state variables */
#define KAFI_DIM_X   (3)    /* number of rows of state vector */
#define KAFI_DIM_Y   (4)   /* number of rows of measurement vector (ACC_X, ACC_Y, ACC_Z, CompassHeading)*/
#define KAFI_DIM_U   (3)    /* number of rows of control vector */

#define SMOOTH_LEN 10
/* Predict the Measurent */
void trPredictMeasurement(void);
/* Update the Jacobi Matrix */
void trUpdateJacobiMatrix(void);
/* Extract measurements and store them in vector matY. 
   Measurement validity is indicated by fYValid[] */
void trMeasure(void);
/* Innovation: calculate Xd, and Pd  */
void trInnovate(void);
/* Update transition matrix Phi  */
void trUpdatePhi(void); 
/* Update control matrix B */
void trUpdateBU(void);
/* Predict new state Xs and Ps */
void KAFIPrediction(void);
/* Main Kalman Filter Loop */
void KalmanFilter(void);
/* Setup the Kalman Filter Parameter */
void Kafi_Init(void);
/* Not done yet */
void trEstimateVelocity(void);
/* Limit Angles to [-2Pi;...;+2Pi] */
void trLimitAngles(void);

typedef struct
{
f32_t Phi;
f32_t Theta;
f32_t Psi;
//i32_t iPhi10;
//i32_t iTheta10;
//i32_t iPsi10;
//f32_t dPhi;
//f32_t dTheta;
//f32_t dPsi;
//f32_t du;
//f32_t dv;
//f32_t dw;
//f32_t X;
//f32_t Y;
//f32_t Z;
} status_t;

typedef struct {
	unsigned char pmax,pmin,pdata;
	float datas[SMOOTH_LEN];
}rawdata_t;

extern status_t FilterdStatus;

/*typedef*/ enum
{
   _p = 0,
   _q,
   _r,
   c_control_variables                       /* number of control variables */
} /*trControlVariables_e*/;


#ifdef USE_Extended_MM3_Measurement_Model
/*typedef*/ enum
{
  _ax = 0,
  _ay,
  _az,
  _mx,
  _my,
  _mz,
  c_observation_variables                       /* number of observation variables */
} /*trObservationVariables_e*/;
#else
/*typedef*/ enum
{
  _ax = 0,
  _ay,
  _az,
  _compass,
  c_observation_variables                       /* number of observation variables */
} /*trObservationVariables_e*/;
#endif

/*typedef*/ enum
{
   _Phi = 0,
   _Theta,
   _Psi,
   //_du,
   //_dv,
   //_dw,
   c_state_variables                       /* number of state variables */
} /*trStateVariables_e*/;
#endif
